#include "rigidbody.h"

using namespace TorPhysix;

TorPhysix::RigidBody::RigidBody()
{
	isStatic = false;
	position.is( 0.0, 0.0, 0.0 );
	orientation.setScalar(1.0);
	orientation.setVector(0.0, 0.0, 0.0);

	velocity.is( 0.0, 0.0, 0.0 );
	rotation.is( 0.0, 0.0, 0.0 );

	acceleration.is( 0.0, 0.0, 0.0 );
	lastFrameAcceleration.is( 0.0, 0.0, 0.0 );

	linearDamping = (real)0.95;
	angularDamping =  (real)0.8;

	inverseMass = 1.0 / 1.0;
	Mat3f identityMtx;
	setIdentity( identityMtx );
	setInertiaTensor( identityMtx );

	clearAccumulators();

	for(int i=0; i<16; i++)
		set( transformMatrix, i, 0.0f );

	calculateDerivedData();
}

// essentially quat2mat, but adds translation
// verified correct
static inline void _calculateTransformMatrix(Mat4f &transformMatrix, const Vec3f &pos, const Quatf &orientation)
{
	transformMatrix[0] = 1-2*orientation.y()*orientation.y()-2*orientation.z()*orientation.z();
	transformMatrix[1] = 2*orientation.x()*orientation.y() -2*orientation.w()*orientation.z();
	transformMatrix[2] = 2*orientation.x()*orientation.z() +2*orientation.w()*orientation.y();
	transformMatrix[3] = pos.x();

	transformMatrix[4] = 2*orientation.x()*orientation.y() + 2*orientation.w()*orientation.z();
	transformMatrix[5] = 1-2*orientation.x()*orientation.x()-2*orientation.z()*orientation.z();
	transformMatrix[6] = 2*orientation.y()*orientation.z() -2*orientation.w()*orientation.x();
	transformMatrix[7] = pos.y();

	transformMatrix[8] = 2*orientation.x()*orientation.z() -2*orientation.w()*orientation.y();
	transformMatrix[9] = 2*orientation.y()*orientation.z() +2*orientation.w()*orientation.x();
	transformMatrix[10] = 1-2*orientation.x()*orientation.x()-2*orientation.y()*orientation.y();
	transformMatrix[11] = pos.z();

	// Its unnecessary to set last row as we only transform Vec3f's (not a single Vec4f), except for the OpenGL transformation
	// where we transforms vertices (these are of course Vec4f's). Could use the custom GLTransform func that sets each element.
	transformMatrix[12] = 0;
	transformMatrix[13] = 0;
	transformMatrix[14] = 0;
	transformMatrix[15] = 1;
}

// This code was created by a code generator
// it removes the rotation from iitBody, and applies the body transform and the body orientation to iitWorld
// iitWorld = rotmat * iitBody * rotmat^T
static inline void _transformInertiaTensor(Mat3f &iitWorld,
										   const Mat3f &iitBody,
										   const Mat4f &rotmat)
{
	real t4 = rotmat[0]*iitBody[0]+rotmat[1]*iitBody[3]+rotmat[2]*iitBody[6];
	real t9 = rotmat[0]*iitBody[1]+rotmat[1]*iitBody[4]+rotmat[2]*iitBody[7];
	real t14 = rotmat[0]*iitBody[2]+rotmat[1]*iitBody[5]+rotmat[2]*iitBody[8];
	real t28 = rotmat[4]*iitBody[0]+rotmat[5]*iitBody[3]+rotmat[6]*iitBody[6];
	real t33 = rotmat[4]*iitBody[1]+rotmat[5]*iitBody[4]+rotmat[6]*iitBody[7];
	real t38 = rotmat[4]*iitBody[2]+rotmat[5]*iitBody[5]+rotmat[6]*iitBody[8];
	real t52 = rotmat[8]*iitBody[0]+rotmat[9]*iitBody[3]+rotmat[10]*iitBody[6];
	real t57 = rotmat[8]*iitBody[1]+rotmat[9]*iitBody[4]+rotmat[10]*iitBody[7];
	real t62 = rotmat[8]*iitBody[2]+rotmat[9]*iitBody[5]+rotmat[10]*iitBody[8];

	iitWorld[0] = t4*rotmat[0]+t9*rotmat[1]+t14*rotmat[2];
	iitWorld[1] = t4*rotmat[4]+t9*rotmat[5]+t14*rotmat[6];
	iitWorld[2] = t4*rotmat[8]+t9*rotmat[9]+t14*rotmat[10];
	iitWorld[3] = t28*rotmat[0]+t33*rotmat[1]+t38*rotmat[2];
	iitWorld[4] = t28*rotmat[4]+t33*rotmat[5]+t38*rotmat[6];
	iitWorld[5] = t28*rotmat[8]+t33*rotmat[9]+t38*rotmat[10];
	iitWorld[6] = t52*rotmat[0]+t57*rotmat[1]+t62*rotmat[2];
	iitWorld[7] = t52*rotmat[4]+t57*rotmat[5]+t62*rotmat[6];
	iitWorld[8] = t52*rotmat[8]+t57*rotmat[9]+t62*rotmat[10];
}


void RigidBody::calculateDerivedData()
{
	real d = norm( orientation );
	if( d == 0 )
	{
		orientation.setScalar(1.0);
	}else{
		orientation = normalize(orientation);
	}
	
	_calculateTransformMatrix( transformMatrix, position, orientation );

	// Calculate the inertiaTensor in world space.
	_transformInertiaTensor(inverseInertiaTensorWorld,
		inverseInertiaTensor,
		transformMatrix);
}

static inline void _checkInverseInertiaTensor(const Mat3f &iitWorld)
{
	// TODO: Perform a validity check in an assert.
	// Asserting diagonal is non zero. Each axis has some inertia.
	//assert( iitWorld[0] > 0 && iitWorld[5] > 0 && iitWorld[15] );
}

void RigidBody::setInertiaTensor( const Mat3f &inertiaTensor )
{
	inverseInertiaTensor = inverse( inertiaTensor );
	_checkInverseInertiaTensor( inverseInertiaTensor );
}

void RigidBody::integrate(real dt)
{
	// Linear acceleration from force
	lastFrameAcceleration = acceleration;
	lastFrameAcceleration += forceAccum * inverseMass;

	// Angular acceleration from torque
	Vec3f angularAcc = inverseInertiaTensorWorld * torqueAccum;

	// Integrate velocities
	velocity += lastFrameAcceleration * dt;
	rotation += angularAcc * dt;

	// Drag
	velocity *= real_pow(linearDamping, dt);
	rotation *= real_pow(angularDamping, dt);

	// New position
	position += velocity * dt;
	
	// New angular position
	addScaledVector(orientation, rotation, dt);

	// TODO remove and let world/physics manager handle this transformation?
	calculateDerivedData();
	clearAccumulators();
}

void RigidBody::clearAccumulators()
{
	forceAccum.is( 0.0, 0.0, 0.0 );
	torqueAccum.is( 0.0, 0.0, 0.0 );
}

Vec3f TorPhysix::RigidBody::getPointInLocalSpace( const Vec3f &point )
{
	//Mat4f inverseTform = inverse(transformMatrix);
	//return transform( inverseTform, point );

	// Faster method:
	Vec3f tmp = point;
	// Remove translation
	tmp.x() -= transformMatrix[3];
	tmp.y() -= transformMatrix[7];
	tmp.z() -= transformMatrix[11];
	// Apply rotation
	return Vec3f(
		tmp.x() * transformMatrix[0] +
		tmp.y() * transformMatrix[4] +
		tmp.z() * transformMatrix[8],

		tmp.x() * transformMatrix[1] +
		tmp.y() * transformMatrix[5] +
		tmp.z() * transformMatrix[9],

		tmp.x() * transformMatrix[2] +
		tmp.y() * transformMatrix[6] +
		tmp.z() * transformMatrix[10]
	);
}

Vec3f TorPhysix::RigidBody::getPointInWorldSpace( const Vec3f &point )
{
	// We are interested in orientation and position
	return transform(transformMatrix, point);
}

void TorPhysix::RigidBody::addForceAtPoint( const Vec3f &force, const Vec3f &point )
{
	Vec3f pt = point;
	pt -= position;

	forceAccum += force;
	torqueAccum += cross( pt, force );
}

void RigidBody::addForceAtBodyPoint( const Vec3f &force, const Vec3f &point )
{
	// Convert to coordinates relative to the center of mass
	Vec3f pt = getPointInWorldSpace( point );
	addForceAtPoint( force, pt );
}

void TorPhysix::RigidBody::setPosition( real x, real y, real z )
{
	position.is(x,y,z);
}

void RigidBody::setOrientation( const Quatf newOrientation )
{
	orientation = newOrientation; 
	
	real d = norm( orientation );
	if( d == 0 )
	{
		orientation.setScalar(1.0);
	}else{
		orientation = normalize(orientation);
	}
}

void RigidBody::setStatic( bool isStatic )
{
	RigidBody::isStatic = isStatic;
}


void RigidBody::getGLTransform(float matrix[16]) const
{
	matrix[0] = (float)transformMatrix[0];
	matrix[1] = (float)transformMatrix[4];
	matrix[2] = (float)transformMatrix[8];
	matrix[3] = 0;

	matrix[4] = (float)transformMatrix[1];
	matrix[5] = (float)transformMatrix[5];
	matrix[6] = (float)transformMatrix[9];
	matrix[7] = 0;

	matrix[8] = (float)transformMatrix[2];
	matrix[9] = (float)transformMatrix[6];
	matrix[10] = (float)transformMatrix[10];
	matrix[11] = 0;

	matrix[12] = (float)transformMatrix[3];
	matrix[13] = (float)transformMatrix[7];
	matrix[14] = (float)transformMatrix[11];
	matrix[15] = 1;
}